/*
 * main.cpp
 *
 *  Created on: 18/05/2010
 *      Author: Jorge Antonio García Galicia
 */
#include <iostream>
#include <cstdlib>
#include <cmath>
#include <ctime>
#include <GL/glut.h>

#include "geometry/Vector3D.h"
#include "physics/Particle.h"
#include "physics/EulerIntegrator.h"
#include "physics/RungeKuttaIntegrator.h"
#include "graphics/Sphere.h"

#define MUNDO 100.0f   // Tope de las coordenados de el mundo
#define DT 0.1f

using namespace std;
using namespace geometry;
using namespace physics;
using namespace graphics;

void redimensiona(GLsizei w, GLsizei h);
void dibuja(void);
void inicializa(void);
void tecladoEspecial(int key, int mx, int my);
void funcionTiempo(void);
void moverEscena(void);
void teclado(unsigned char key, int mx, int my);
float grad2Radians(float);
bool colisionMuro(Particle& p, Vector3D& n, float radio);
void respondeMuro(Particle& p, const Vector3D& n);
bool colisionObjetos(Particle& p, Particle& q, Vector3D& n, float rP, float rQ);
void respondeObjetos(Particle& p, Particle& q, const Vector3D& n);

Particle a, b;
Sphere sA(5.0);
Sphere sB(10.0);

float elasticidad, fuerzaDelEmpujon;

int main (int argc, char* argv[]) {

	glutInit(&argc, argv);
    glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA | GLUT_DEPTH);
    glutInitWindowSize(400, 400);
    glutInitWindowPosition(100, 100);
    glutCreateWindow("Deteccion y respuesta de colisiones");
    inicializa();
    /** Funciones de callback */
    glutSpecialFunc(tecladoEspecial);
    glutDisplayFunc(dibuja);
    glutReshapeFunc(redimensiona);
    glutKeyboardFunc(teclado);
    glutIdleFunc(funcionTiempo);
    glutMainLoop();

    return 0;
}

void inicializa(void)
{
     glClearColor(0.0, 0.0, 0.0, 0.0);
     glEnable(GL_DEPTH_TEST);
     Vector3D cero(0, 0, 0);
     glLineWidth(1.0);
     srand(time(NULL));
     elasticidad = 0.85;
     fuerzaDelEmpujon = 10.0;

     //Inicializamos las particulas
     a.setPosition(Vector3D(-2.0 * MUNDO / 3.0, 0, 0));
     a.setVelocity(cero);
     a.setForce(cero);
     a.setMass(1.0);

     b.setPosition(Vector3D(2.0 * MUNDO / 3.0, 0, 0));
     b.setVelocity(cero);
     b.setForce(cero);
     b.setMass(2.0);

     //Inicializamos los objetos graficos
     sA.setColor(Vector3D(0, 1, 0));
     sB.setColor(Vector3D(0, 0, 1));

     //Prendemos las luces
     glEnable(GL_LIGHTING);
     glEnable(GL_LIGHT0);
     glEnable(GL_COLOR_MATERIAL);
     glColorMaterial(GL_FRONT, GL_AMBIENT_AND_DIFFUSE);
}

void redimensiona(GLsizei w, GLsizei h) {
    GLfloat aspectRatio;

    if (h == 0)
       h = 1;

    glViewport(0, 0, w, h);

    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();

    aspectRatio = (GLfloat)w / (GLfloat)h;

    if (w <= h)
       glOrtho(-MUNDO, MUNDO, -MUNDO / aspectRatio, MUNDO / aspectRatio, -MUNDO, MUNDO);
    else
       glOrtho(-MUNDO * aspectRatio, MUNDO * aspectRatio, -MUNDO, MUNDO, -MUNDO, MUNDO);

    glMatrixMode(GL_MODELVIEW);
    glLoadIdentity();
}

void tecladoEspecial(int key, int mx, int my) {
	switch(key) {
		case GLUT_KEY_LEFT:

		break;

		case GLUT_KEY_RIGHT:

		break;

		case GLUT_KEY_UP:

		break;

		case GLUT_KEY_DOWN:

		break;

		case GLUT_KEY_HOME:

		break;
	}

	glutPostRedisplay();
}

void funcionTiempo() {
	glutPostRedisplay();
}

void dibuja () {
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	moverEscena();
	sA.setCenter(a.getPosition());
	sB.setCenter(b.getPosition());

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();

	sA.draw();
	sB.draw();

	//Dibujar la barrera de la colsion
	glColor3f(1.0, 0.0, 0.0);
	glBegin(GL_LINE_LOOP);
		glVertex3f(MUNDO, MUNDO, 0.0);
		glVertex3f(-MUNDO, MUNDO, 0.0);
		glVertex3f(-MUNDO, -MUNDO, 0.0);
		glVertex3f(MUNDO, -MUNDO, 0.0);
	glEnd();

	glutSwapBuffers();

}

void moverEscena() {
	EulerIntegrator e(DT);
	e.integrate(a);
	e.integrate(b);

	b.emptyForce();
	a.emptyForce();

	//Detectar y resolver colsiones aqui
	Vector3D n;

	if (colisionMuro(a, n, sA.getRadio())) {
		respondeMuro(a, n);
	}

	if (colisionMuro(b, n, sB.getRadio())) {
		respondeMuro(b, n);
	}

	if (colisionObjetos(a, b, n, sA.getRadio(), sB.getRadio())) {
		respondeObjetos(a, b, n);
	}
}

void teclado(unsigned char key, int mx, int my) {
	float angle;
	Vector3D empujon;

	switch(key) {

	case 27:
		exit(0);
		break;

	case 's':
		angle = rand() % 360;
		empujon = Vector3D(cos(grad2Radians(angle)), sin(grad2Radians(angle)), 0.0);
		a.emptyVelocity();
		a.emptyForce();
		a.addForce(fuerzaDelEmpujon * empujon);
		angle = rand() % 360;
		empujon = Vector3D(cos(grad2Radians(angle)), sin(grad2Radians(angle)), 0.0);
		b.emptyVelocity();
		b.emptyForce();
		b.addForce(fuerzaDelEmpujon * empujon);
	break;

	case 'c':
		angle = rand() % 360;
		empujon = Vector3D(cos(grad2Radians(angle)), sin(grad2Radians(angle)), 0.0);
		a.addForce(fuerzaDelEmpujon * empujon);
		angle = rand() % 360;
		empujon = Vector3D(cos(grad2Radians(angle)), sin(grad2Radians(angle)), 0.0);
		b.addForce(fuerzaDelEmpujon * empujon);
	break;

	case '+':
		fuerzaDelEmpujon += 2.0;
	break;

	case '-':
		if (fuerzaDelEmpujon > 2.0) {
			fuerzaDelEmpujon -= 1.0;
		}
	break;


	}

	glutPostRedisplay();
}

float grad2Radians (float angleDeg) {
	return 0.017453293f * angleDeg;
}

bool colisionMuro(Particle& c, Vector3D& n, float radio) {
Vector3D p = c.getPosition(), ajuste;

	if (p.getX() + radio >= MUNDO) {
		n = Vector3D(-1, 0, 0);
		ajuste = Vector3D(MUNDO - radio, p.getY(), p.getZ());
		c.setPosition(ajuste);
		return true;
	} else if (p.getX() - radio <= -MUNDO) {
		n = Vector3D(1, 0, 0);
		ajuste = Vector3D(-MUNDO + radio, p.getY(), p.getZ());
		c.setPosition(ajuste);
		return true;
	} else if (p.getY() + radio >= MUNDO) {
		n = Vector3D(0, -1, 0);
		ajuste = Vector3D(p.getX(), MUNDO - radio, p.getZ());
		c.setPosition(ajuste);
		return true;
	} else if (p.getY() - radio <= -MUNDO) {
		n = Vector3D(0, 1, 0);
		ajuste = Vector3D(p.getX(), -MUNDO + radio, p.getZ());
		c.setPosition(ajuste);
		return true;
	} else {
		return false;
	}
}

void respondeMuro(Particle& p, const Vector3D& n) {
	Vector3D s, u_n, u_t, u;
	u = p.getVelocity();
	u_n = u.parteNormal(n);
	u_t = u - u_n;
	s = -u_n + u_t;
	p.setVelocity(s * elasticidad);
}

bool colisionObjetos(Particle& p, Particle& q, Vector3D& n, float rP, float rQ) {
	Vector3D r = p.getPosition() - q.getPosition();
	if (r.largo() <= (rP + rQ)) {
		n = r;
		n.normalize();
		p.setPosition(q.getPosition() + ((rP + rQ) * n));
		return true;
	} else {
		return false;
	}
}

void respondeObjetos(Particle& p, Particle& q, const Vector3D& n) {
	Vector3D u, v, s, w, u_n, u_t, s_n, w_n;
	float r = p.getMass() / q.getMass();

	u = p.getVelocity();
	v = q.getVelocity();

	u -= v;
	u_n = u.parteNormal(n);
	u_t = u - u_n;
	s_n = ((r - 1) / (r + 1)) * u_n;
	w_n = ((2.0 * r) / (r + 1)) * u_n;
	s = u_t + s_n + v;
	w = w_n + v;

	p.setVelocity(s);
	q.setVelocity(w);
}
